#include "filter.h"

#include <cassert>
#include <list>
#include <cmath>

using namespace pip;
using namespace std;

/**
 * Convert a Float image to an Unsigned Char image and scales the values between 0 and 255.
 * A constant float image is filled with value 128.
 *
 * @param input Constant reference to the input Float image
 * @param output Reference to the output Unsigned Char image
 */
void convertFloatToUnsignedChar(const Image<float> &input, Image<unsigned char> &output)
{
  float min, max;
  getMinMaxImage(input, min, max);

  output.resize(input.dimension());

  if (min == max) {
    output.fill(128);
    return;
  }
  
  float scale(255.0f / (max - min));

  for (int z = 0; z < input.dim(2); ++z) {
    for (int y = 0; y < input.dim(1); ++y) {
      for (int x = 0; x < input.dim(0); ++x) {
        output(x, y, z) = (unsigned char)(scale * (input(x, y, z) - min));
      }
    }
  }
}

/**
 * Converts an Unsigned Char image to a Float image.
 *
 * @param input Constant reference to the input Unsigned Char image
 * @param output Reference to the output Float image
 */
void convertUnsignedCharToFloat(const Image<unsigned char> &input, Image<float> &output)
{
  output.resize(input.dimension());

  for (int z = 0; z < input.dim(2); ++z) {
    for (int y = 0; y < input.dim(1); ++y) {
      for (int x = 0; x < input.dim(0); ++x) {
        output(x, y, z) = (float)input(x, y, z);
      }
    }
  }
}

/**
 * Convolves the input image with a kernel and stores the result in the output.
 *
 * @param input Constant reference to the input image
 * @param output Reference to the output image
 * @param mask Constant reference to the kernel
 */
void filter(const Image<float> &input, Image<float> &output, const Image<float> &mask, const BorderInfo &bi)
{
  // Only 2D n x n kernels, with n is odd.
  if (mask.dim(0) != mask.dim(1) || mask.dim(0) % 2 == 0 || mask.nrDims() != 2) {
    cerr << "Given kernel is incorrect. It is either not squared, not odd-sized, or not 2 dimensional." << endl;
    return;
  }

  int centre((mask.dim(0) - 1) / 2);
  Image<float> temp = input.bordered(centre, bi);

  output.resize(input.dimension());
  output.fill(0);

  // Loop over all pixels
  for (int y = 0; y < input.dim(1); ++y) {
    for (int x = 0; x < input.dim(0); ++x) {
      // Loop over all kernel values of mask
      for (int dy = -centre; dy <= centre; ++dy) {
        for (int dx = -centre; dx <= centre; ++dx) {
          output(x, y) += temp(x + dx, y + dy) * mask(centre - dx, centre - dy);
        }
      }
    }
  }
}

/**
 * Performs a separable convolution. The first dimension is convolved with the first
 * kernel in kernels, and so on.
 *
 * @param in Input image
 * @param out Output image
 * @param kernels List of kernels
 * @param bi Border information
 */
void filterSeparable(const Image<float> &in, Image<float> &out, const vector<vector<float> > &kernels, const BorderInfo &bi)
{
  if (in.nrDims() != kernels.size()) {
    cerr << "Incorrect dimensions." << endl;
    return;
  }

  // Find out what the largest kernel half size is,
  // we need a border of this size
  int n(0);
  for (int i = 0; i < (int)kernels.size(); ++i) {
    if ((int)kernels[i].size() / 2 > n) {
      n = kernels[i].size() / 2;
    }
  }

  Image<float> temp1(in.bordered(n, bi));
  Image<float> temp2(in.dimension(), n, bi);
  temp2.fill(0);
  out.resize(in.dimension());
  out.fill(0);

  // First along x-direction,
  // store result in temp2
  vector<float> kernel = kernels[0];
  n = kernel.size() / 2;
  for (int y = 0; y < in.dim(1); ++y) {
    for (int x = 0; x < in.dim(0); ++x) {
      for (int i = -n; i <= n; ++i) {
        temp2(x, y) += temp1(x - i, y) * kernel[i + n];
      }
    }
  }

  // Now along y-direction,
  // store result in out
  kernel = kernels[1];
  n = kernel.size() / 2;
  for (int y = 0; y < in.dim(1); ++y) {
    for (int x = 0; x < in.dim(0); ++x) {
      for (int i = -n; i <= n; ++i) {
        out(x, y) += temp2(x, y - i) * kernel[i + n];
      }
    }
  }
}

/**
 * Samples the unnormalized Gaussian for given sigma at integer values.
 *
 * @param i
 * @param sigma
 * @return Unnormalized Gaussian
 */
float g0(const int &i, const float &sigma)
{
  return (float)(exp(-(float)(i * i) / (2.0 * sigma * sigma)));
}

/**
 * Samples the unnormalized first derivative of the Gaussian for given sigma at integer values.
 *
 * @param i
 * @param sigma
 * @return Unnormalized first derivative of the Gaussian
 */
float g1(const int &i, const float &sigma)
{
  return (float)(-(float)i / (sigma * sigma) 
    * exp(-(float)(i * i) / (2.0 * sigma * sigma)));
}

/**
 * Samples the unnormalized second derivative of the Gaussian for given sigma at integer values.
 *
 * @param i
 * @param sigma
 * @return Unnormalized second derivative of the Gaussian
 */
float g2(const int &i, const float &sigma)
{
  return (float)(1.0 / (sigma * sigma) * ((float)(i * i) / (sigma * sigma) - 1)
    * exp(-(float)(i * i) / (2.0 * sigma * sigma)));
}

/**
 * Samples the unnormalized third derivative of the Gaussian for given sigma at integer values.
 *
 * @param i
 * @param sigma
 * @return Unnormalized third derivative of the Gaussian
 */
float g3(const int &i, const float &sigma)
{
  return (float)(1.0 / (sigma * sigma * sigma * sigma) * ((3.0 * (float)(i) - (float)(i * i * i) / (sigma * sigma)))
    * exp(-(float)(i * i) / (2.0 * sigma * sigma)));
}

/**
 * Normalizes the vector to unit absolute sum.
 *
 * @param kernel Kernel to normalize
 */
void normalize(vector<float> &kernel)
{
  float f(0.0);
  for (unsigned int i = 0; i < kernel.size(); ++i) {
    f += fabs(kernel[i]);
  }
  
  if (f > 0) {
    for (unsigned int i = 0; i < kernel.size(); ++i) {
      kernel[i] /= f;
    }
  }
}

/**
 * Computes a Gaussian derivative of scale sigma and order dx, dy of in, result stored in out.
 *
 * @param in Input image
 * @param out Output image
 * @param sigma
 * @param dx
 * @param dy
 * @param cutoff
 * @param bi Border information
 */
void gauss(const Image<float> &in, Image<float> &out, const float &sigma, const int &dx, const int &dy, const float &cutoff, const BorderInfo &bi)
{
  //determine length of kernels based on sigma and cutoff
  int n((int)(sigma * cutoff));

  //fill kernels. If dx or dy are not 0,1,2 or 3 give an error and return
  if (dx < 0 || dx > 3 || dy < 0 || dy > 3) {
    return;
  }

  vector<vector<float> > kernels(2);
  for (int i = -n; i <= n; ++i) {
    switch(dx) {
      case 0:
        kernels[0].push_back(g0(i, sigma));
        break;
      case 1:
        kernels[0].push_back(g1(i, sigma));
        break;
      case 2:
        kernels[0].push_back(g2(i, sigma));
        break;
      case 3:
        kernels[0].push_back(g3(i, sigma));
        break;
      default:
        break;
    }

    switch(dy) {
      case 0:
        kernels[1].push_back(g0(i, sigma));
        break;
      case 1:
        kernels[1].push_back(g1(i, sigma));
        break;
      case 2:
        kernels[1].push_back(g2(i, sigma));
        break;
      case 3:
        kernels[1].push_back(g3(i, sigma));
        break;
      default:
        break;
    }
  }

  //normalize the kernels
  normalize(kernels[0]);
  normalize(kernels[1]);

  //call filterseparable
  filterSeparable(in, out, kernels, bi);
}
